#include "World.h"
#include "Box2D/Box2D.h"
#include "Common/Utility.h"
#include <iostream>

using namespace std;
using namespace irr;
using namespace scene;
using namespace core;
using namespace video;

#define		DEG2RAD		3.14 / 180

CWorld*					CWorld::m_ptrInstance = NULL;
static vector<b2Vec2>	t_categoryPnt;
static vector<SColor>	t_colorList;

CWorld::CWorld()
{
	m_bFlagCreate = false;

	float gap = 32.72f;
	float x, y;

	for(int i = 0; i < 11; i++)
	{
		float deg = i * gap * DEG2RAD;
		x = cos(deg) * 120;
		y = sin(deg) * 120;

		t_categoryPnt.push_back(b2Vec2(x, y));
	}

	t_categoryPnt.push_back(b2Vec2(0, 0));			// query

	t_colorList.push_back(SColor(180, 255, 50, 50));
	t_colorList.push_back(SColor(180, 50, 255, 50));
	t_colorList.push_back(SColor(180, 50, 50, 255));
	t_colorList.push_back(SColor(180, 200, 255, 150));
	t_colorList.push_back(SColor(180, 255, 150, 255));
	t_colorList.push_back(SColor(180, 150, 255, 255));
	t_colorList.push_back(SColor(180, 150, 50, 255));
	t_colorList.push_back(SColor(180, 100, 100, 100));
	t_colorList.push_back(SColor(180, 255, 200, 200));
	t_colorList.push_back(SColor(180, 255, 255, 200));
	t_colorList.push_back(SColor(180, 200, 200, 255));
	t_colorList.push_back(SColor(180, 255, 255, 255));
}

CWorld::~CWorld()
{
	if(m_bFlagCreate)
		Destroy();
}

int CWorld::Create(float timeStep, int velIteration, int posIteration)
{
	if(m_bFlagCreate)
		return ALREADY_CREATE;

	// create renderer
	m_ptrRenderer = CRenderer::Instance();
		
	if(m_ptrRenderer->Create() != SUCCESS)
		return FAIL_CREATERENDERER;

	m_ptrRenderer->AddLight(irr::core::vector3df(0, 0, 200), 
							irr::video::SColorf(0.5f, 0.5f, 0.5f), 300);

	m_ptrRenderer->AddLight(irr::core::vector3df(0, 0, 100), 
						irr::video::SColorf(0.2f, 0.2f, 0.2f), 300);

	m_ptrRenderer->AddLight(irr::core::vector3df(-100, 0, 200), 
							irr::video::SColorf(0.4f, 0.4f, 0.4f), 300);

	m_ptrRenderer->AddDefaultCamera();

	// create world
	m_ptrWorld = new b2World(b2Vec2(0.f, 0.f));

	m_timeStep				= timeStep;
	m_velocityIterations	= velIteration;
	m_positionIteration		= posIteration;

	m_bFlagCreate = true;

	return SUCCESS;
}

int CWorld::Destroy()
{
	if(!m_bFlagCreate)
		return NOT_CREATE;

	m_ptrRenderer->Destroy();

	m_bFlagCreate = false;

	return SUCCESS;
}

CWorld* CWorld::Instance()
{
	if(!m_ptrInstance)
		m_ptrInstance = new CWorld;

	return m_ptrInstance;
}

int CWorld::AddDynSphere(std::string name, int nId, rVector3 pos, float radius, CATEGORY cat)
{
	b2CircleShape	circle;
	b2BodyDef		bd;
	b2FixtureDef	fd;

	if(m_bodyList[name])
		return OBJECT_ALREADYEXIST;

	circle.m_radius = (cat == QUERY ? 0.01 : radius);

	bd.type = b2_dynamicBody;
	bd.position.Set(pos.x, pos.y);
	fd.shape = &circle;
	fd.restitution = 1.f;

	b2Body*		pBody = m_ptrWorld->CreateBody(&bd);
	ISceneNode*	pNode = 0;

	pBody->CreateFixture(&fd);
	pNode = m_ptrRenderer->AddSphere(name, nId, vector3df(pos.x, pos.y, pos.z), 
									 1.5f, t_colorList[cat]);
	if(cat != QUERY)
		m_ptrRenderer->AddSceneNode2Cat(cat, pNode);
	stPair* pPair = new stPair(pBody, pNode, cat);
	m_bodyList[name] = pPair;

	return SUCCESS;
}

void CWorld::Step()
{
	if(!m_bFlagCreate)
		return ;

	static unsigned short	oldTime = 0;
	unsigned short			curTime = CUtility::MilliSec();
	int						nFps	= 0;
	stringw					str		= L"Visualizer - ";

	if(abs(curTime - oldTime) > 50)
	{
		hash_map<std::string, stPair*>::iterator itr;

		m_ptrWorld->Step(m_timeStep, m_velocityIterations, m_positionIteration);

		// update position
		for(itr = m_bodyList.begin(); itr != m_bodyList.end(); ++itr)
		{
			float  sim = m_queryResult[itr->first] * 3;
			float  ratio = (1 - sim) < 0.2 ? 0.2 : 1 - sim;
			ratio = ratio > 0.5 ? 0.5 : ratio;
			b2Vec2 pos = itr->second->pBody->GetPosition();
			b2Vec2 org = sim == 0 ? 
						 t_categoryPnt[itr->second->cat] : 
						 b2Vec2(t_categoryPnt[itr->second->cat].x * ratio,
								t_categoryPnt[itr->second->cat].y * ratio);
			b2Vec2 diff;

			t_ComputeForce(diff.x, org.x, pos.x);
			t_ComputeForce(diff.y, org.y, pos.y);
			
			itr->second->pNode->setPosition(vector3df(pos.x, pos.y, 0));
			itr->second->pBody->SetTransform(b2Vec2(pos.x + diff.x, pos.y + diff.y), 0);
		}

		m_ptrRenderer->Step();
	}
	else
		m_ptrRenderer->Device()->yield();
}

void CWorld::t_ComputeForce(float& ret, float a, float b)
{
	if(a - b < 0)
	{
		if(abs(a - b) > 30)			ret = -0.3;
		else if(abs(a - b) > 20)	ret = -0.2;
		else if(abs(a - b) > 10)	ret = -0.1;
		else						ret = 0;
	}
	else
	{
		if(abs(a - b) > 30)			ret = 0.3;
		else if(abs(a - b) > 20)	ret = 0.2;
		else if(abs(a - b) > 10)	ret = 0.1;
		else						ret = 0;
	}
}




